/**
 * @file RoboDecision.cpp
 * @brief 机器人视觉控制类，为整个代码的总类，由Modules中的不同组件构成
 *        可分为以下四个部分：
 *        感知器：Perception
 *        识别器：Detector
 *        决策器：Strategy
 *        姿态解算器：PoseSolver
 *        指令解析器：CommandParser
 *        数据收发器：SerialPort
 *        实例化后自动调用不同机构完成视觉控制流程
 * @author Rosen (1018477962@qq.com)
 * @version 1.0
 * @date 2021-09-26
 * 
 * @copyright Copyright (c) 2021 by Rosen.
 */
#include "RoboDecision.h"
#include <librealsense2/rs.hpp>

/**
 * @brief Construct a new Robo Decision:: Robo Decision object
 *        使用单例型设计模式，整个机器人识别决策过程都集中在该构造函数中实现
 *        本设计模式将整个机器人视觉部分作为一个总类，有四个部分组成：
 *        感知器、识别器、决策器、姿态解算器、指令解析器、数据收发器
 *        由单一职责原则，每个部分相互独立，互相配合，完成整个识别决策过程
 */
RoboDecision::RoboDecision(const int &argc, char **argv, const char *keys)
{
    // 机器人初始化
    this->RoboInit(argc, argv, keys);
    // 打印帮助信息
    this->_p_commandparser->if_PrintHelp();
    // 初始化感知器状态
    std::string source = this->_p_commandparser->get<std::string>("input");
    this->_p_perception->InitState(source);

    while (true)
    {
        // 输入"-time"，设置计时器
        this->_p_commandparser->if_Set_Counter();
        // 获取RGB图像
        cv::Mat frame = this->_p_perception->obtain_RGBImage();
        // 获取深度图像
        cv::Mat depth = this->_p_perception->obtain_DepthImage();
        // 识别器执行推理
        std::vector<Object> objects = this->_p_detector->inference(frame, cv::Size(640, 640), depth);
        // 运行决策器
        this->_p_strategy->analysis(objects);
        if (this->_p_strategy->isDiscover)
            this->_p_posesolver->resolute(frame, depth, this->_p_perception->getDepIntri(), this->_p_strategy->getGoal().rect);

        // 输入"--show"，决定是否绘制
        if (this->_p_commandparser->get<bool>("show"))
            this->_p_detector->drawBox(frame);
        // 输入"-show"，显示图像
        this->_p_commandparser->if_Show(frame);
        this->_p_commandparser->if_Show(depth);
        // 传入类成员函数指针，调用输出信息函数
        void (Strategy::*ptr_fun)();
        ptr_fun = &Strategy::info;
        this->_p_commandparser->if_Output(*this->_p_strategy, ptr_fun);

        // 输入"-time"，计算运行帧率
        this->_p_commandparser->if_CountingFPS();
    }
}

/**
 * @brief 机器人初始化函数，当
 * 
 * @param argc 命令行参数数量
 * @param argv 命令行参数指令
 * @param key  定义可操作参数
 */
void RoboDecision::RoboInit(const int &argc, char **argv, const char *key)
{
    // 创建感知器
    this->_p_perception = p_perception(new FrameProcessor(RS2_STREAM_COLOR));
    // 创建识别器
    std::string modelPath = roboParam.modelPath;
    this->_p_detector = p_detector(new Detector(modelPath));
    // 创建决策器
    this->_p_strategy = p_strategy(new Strategy());
    // 创建姿态解算器
    this->_p_posesolver = p_posesolver(new PoseSolver());
    // 创建指令解析器
    this->_p_commandparser = p_commandparser(new CommandParser(argc, argv, key));
    // 创建数据收发器
    this->_p_serialport = p_serialport(new SerialPort());
}
